#! /usr/bin/env python2
# -* - coding: utf-8 -*

import rospy
import tf
from arm_controller.msg import control
from ar_track_alvar_msgs.msg import AlvarMarkers
from geometry_msgs.msg import TransformStamped
from math import pow,sqrt

class eye_hand():
    def __init__(self):
        rospy.init_node('eye_hand')
        rospy.Subscriber('ar_pose_marker',AlvarMarkers,self.marker_callback)
        rospy.Subscriber('arm_controller/position_info',control,self.pos_callback)
        rospy.Subscriber('marker',TransformStamped,self.send_marker)
        rospy.Subscriber('camera',TransformStamped,self.send_cam)
        rospy.Subscriber('start_end',TransformStamped,self.send_end)
        self.pub_marker = rospy.Publisher('marker',TransformStamped,queue_size=10)
        self.pub_cam = rospy.Publisher('camera',TransformStamped,queue_size=10)
        self.pub_end = rospy.Publisher('start_end',TransformStamped,queue_size=10)
        self.listener = tf.TransformListener() #tf监听
        self.br = tf.TransformBroadcaster() #tf发布
        self.ar_num = 0
        self.rate = rospy.Rate(10.0) # 设置tf频率
        self.mk_pose = control()
        self.get_end = control() #-1.57079632679 0 3.1415926
        self.get_marker = AlvarMarkers()
        self.listener.waitForTransform("/end", "/robot", rospy.Time(0), rospy.Duration(10.0))

    def marker_callback(self,msg):
        """订阅ar码的信息"""
        self.get_marker = msg

    def pos_callback(self,msg):
        """订阅手臂末端坐标"""
        self.get_end = msg

    def send_cam(self,msg):
        """发布相机坐标"""
        x = msg.transform.translation.x
        y = msg.transform.translation.y
        z = msg.transform.translation.z
        q_x = msg.transform.rotation.x
        q_y = msg.transform.rotation.y
        q_z = msg.transform.rotation.z
        q_w = msg.transform.rotation.w
        self.br.sendTransform((x,y,z),
                                                    (q_x,q_y,q_z,q_w),
                                                    msg.header.stamp,
                                                    "cam_"+str(self.ar_num),
                                                    msg.child_frame_id)

    def send_marker(self,msg):
        """发布ar码坐标"""
        x = msg.transform.translation.x
        y = msg.transform.translation.y
        z = msg.transform.translation.z
        q_x = msg.transform.rotation.x
        q_y = msg.transform.rotation.y
        q_z = msg.transform.rotation.z
        q_w = msg.transform.rotation.w
        self.br.sendTransform((x,y,z),
                                                    (q_x,q_y,q_z,q_w),
                                                    msg.header.stamp,
                                                    msg.child_frame_id,
                                                    "robot")

    def send_end(self,msg):
        """发布end码坐标"""
        x = msg.transform.translation.x
        y = msg.transform.translation.y
        z = msg.transform.translation.z
        q_x = msg.transform.rotation.x
        q_y = msg.transform.rotation.y
        q_z = msg.transform.rotation.z
        q_w = msg.transform.rotation.w
        self.br.sendTransform((x,y,z),
                                                    (q_x,q_y,q_z,q_w),
                                                    msg.header.stamp,
                                                    msg.child_frame_id,
                                                    "robot")

    def cal_err(self,ar_num,cam_pose,ar_robot):
        """计算误差"""
        self.ar_num = ar_num
        mk_name = "marker"+str(ar_num)
        time = rospy.Time.now()
        # 手臂坐标
        end_tf = TransformStamped()
        end_pose = self.curr_pose
        end_tf.header.stamp = time
        end_tf.transform.translation = end_pose[0]
        end_tf.transform.rotation = end_pose[1]
        end_tf.child_frame_id = "start_end"
        
        # ar码坐标消息
        mk_tf = TransformStamped()
        mk_pose = self.listener_tf("robot","end")
        mk_tf.header.stamp = time
        print(mk_pose[0])
        mk_tf.transform.translation = mk_pose[0]
        mk_tf.transform.rotation = ar_robot[1]
        mk_tf.child_frame_id = mk_name

        # 相机相对坐标
        cam_tf = TransformStamped()
        cam_tf.header.stamp = time
        cam_tf.transform.translation = cam_pose[0]
        cam_tf.transform.rotation = cam_pose[1]
        cam_tf.child_frame_id = mk_name

        self.pub_end.publish(end_tf)
        self.pub_marker.publish(mk_tf)
        self.pub_cam.publish(cam_tf)
        pos_err,tf_q = self.listener_tf("start_end","cam_"+str(self.ar_num),time=time)
        print("error:>>>"+str(pos_err))
        return pos_err

    def listener_tf(self,child,parent,time=rospy.Time(0)):
        """监听tf"""
        try:
            # 等待变换
            self.rate.sleep()     
            self.listener.waitForTransform(child, parent, time, rospy.Duration(10.0))
            (trans, rot) = self.listener.lookupTransform(child, parent, time)
            tf_trans = TransformStamped().transform.translation
            tf_trans.x = trans[0]
            tf_trans.y = trans[1]
            tf_trans.z = trans[2]
            tf_rot = TransformStamped().transform.rotation
            tf_rot.x = rot[0]
            tf_rot.y = rot[1]
            tf_rot.z = rot[2]
            tf_rot.w = rot[3]
            return tf_trans,tf_rot
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            print("did not find the  tf "+parent + "and" + child)
            # return (0,0,0)
            

    def get_param(self,num):
        point = control()
        point.position.x = rospy.get_param("/eye_on_hand/ar_marker_"+str(num)+"/x")/1000
        point.position.y = rospy.get_param("/eye_on_hand/ar_marker_"+str(num)+"/y")/1000
        point.position.z = rospy.get_param("/eye_on_hand/ar_marker_"+str(num)+"/z")/1000
        return point.position

    def mse(self,a,b):
        err = sqrt(pow(a-b,2))



if __name__=="__main__":
    pos_err = []
    n = eye_hand()

    while not rospy.is_shutdown():
        # marker = n.get_marker.markers[0]
        
        # n.cal_err(marker.id,mk_pose,marker.pose.pose.position)
        
        print("移动机器人，使ar码出现在画面中")
        key = raw_input("移动机器人，使ar码出现在画面中\n输入“m”开始，输入“q”结束\n>>>")
        if(key == "m"):
            cam_pose = [] # 当前相机的位置（相对与ar码）
            ar_robot = [] #ar码的假设位置
            markers = n.get_marker.markers
            
            for marker in markers:
                cam_pose.append(n.listener_tf ("ar_marker_"+str(marker.id),"hand_cam"))
                ar_robot.append(n.listener_tf("robot","ar_marker_"+str(marker.id)))
            n.curr_pose = n.listener_tf("robot","end")
            print("记录“end”当前位置为:"+ str(n.curr_pose))
            print("识别到了(" + str(len(markers)) + ")个ar码")
            i = 0
            for marker in markers:
                print("移动机器人到 ar_marker_"+str(marker.id))
                print("识别到的位置")
                key = raw_input("\n输入“m”继续，输入“q”结束\n>>>")
                if key=="m":
                    pos_err.append(n.cal_err(marker.id,cam_pose[i],ar_robot[i]))
                elif key =="q":
                    break
                i = i +1
                print("------------------------------------------------------------------") #+"百分之"+str(100/marker.id*i))
            x = 0
            y = 0
            z = 0
            for i in pos_err:
                x = x + i.x/len(pos_err)
                y = y + i.y/len(pos_err)
                z = z + i.z/len(pos_err)
            print(x,y,z)
            break
        elif key == "q":
            break
